#include "FindPathAstar.h"
#include <cstdlib>
#include <cassert>
#include <string>

CNode::CNode()
{

}
CNode::~CNode()
{

}
FindPathAstar::FindPathAstar(void)
{

}

FindPathAstar::~FindPathAstar(void)
{

}
void FindPathAstar::SetObstructionMatrix(int xsizeMap, unsigned int map[50][50],unsigned int wall,unsigned int players,unsigned int start,unsigned int goal)
{
	sizeMap = xsizeMap;
	for(int i = 0; i < sizeMap; i++)
	{
		for(int j = 0; j < sizeMap; j++)
		{
			if(map[i][j] == wall)
			{
				obstruction[i][j] = 0;
			}
			else if(map[i][j] == players)
			{
				obstruction[i][j] = 1;
			}
			else if(map[i][j] == start)
			{
				obstruction[i][j] = 2;
				start_x = i;
				start_y = j;
			}
			else if(map[i][j] == goal)
			{
				obstruction[i][j] = 3;
				goal_x = i;
				goal_y = j;
			}
			else
			{
				obstruction[i][j] = 4;
			}
		}
	}
}
void FindPathAstar::InsertIntoOpenList(int x, int y)
{

	if(x >= 0 && x < sizeMap && y >= 0 && y < sizeMap && ClosedList.size() > 0 && !IsElementExits(x,y))
	{
		NODE tempNode;

		if(obstruction[x][y] == 0)
		{
			int base_cost, cost_to_start, cost_to_goal;

			base_cost = obstruction[x][y];
			cost_to_goal = abs(x - goal_x) + abs(y - goal_y);
			cost_to_start= ClosedList.back().StartCost + base_cost;

			tempNode.StartCost = cost_to_start;
			tempNode.TotalCost = base_cost+cost_to_goal+cost_to_start;
			tempNode.X = x;
			tempNode.Y = y;
			tempNode.ParentX = ClosedList.back().X;
			tempNode.ParentY = ClosedList.back().Y;

			// add to open list
			OpenList.push_back(tempNode);
		}
		else if(obstruction[x][y] == 3)
		{
			tempNode.StartCost = 0;
			tempNode.TotalCost = 0;
			tempNode.X = x;
			tempNode.Y = y;
			tempNode.ParentX = ClosedList.back().X;
			tempNode.ParentY = ClosedList.back().Y;

			// add to open list
			OpenList.push_back(tempNode);
		}

	}
}
void FindPathAstar::InsertIntoClosedList(int total_cost, int start_cost, int x, int y)
{
	if(x < sizeMap && x >= 0 && y < sizeMap && y >= 0 && !IsElementExits(x, y))
	{
		NODE tempNode;

		tempNode.StartCost = start_cost;
		tempNode.TotalCost = total_cost;
		tempNode.X = x;
		tempNode.Y = y;
		tempNode.ParentX = x;
		tempNode.ParentY = y;
		ClosedList.push_back(tempNode);		
	}
}

void FindPathAstar::Create()
{
	//Clear previous lists
	ClearAll();

	//add the start node to the closed list
	InsertIntoClosedList(0, 0, start_x, start_y);

	int next_x = start_x;
	int next_y = start_y;
	while(obstruction[next_x][next_y] != 3 && next_x < sizeMap - 1 && next_x >= 0 && next_y < sizeMap - 1 && next_y >= 0)
	{
		//take all the neighboring passable nodes and add them to the OpenList
		InsertIntoOpenList(next_x + 1, next_y);
		InsertIntoOpenList(next_x + 1, next_y + 1);
		InsertIntoOpenList(next_x, next_y + 1);		
		if(next_x > 0)
		{
			InsertIntoOpenList(next_x - 1, next_y + 1);
			InsertIntoOpenList(next_x - 1, next_y);
		}
		if(next_x > 0 && next_y > 0)
		{
			InsertIntoOpenList(next_x - 1, next_y - 1);
		}
		if(next_y > 0)
		{
			InsertIntoOpenList(next_x, next_y - 1);
			InsertIntoOpenList(next_x + 1, next_y - 1);
		}
		

		//find the min node

		if(MinNode(OpenList).StartCost != -1)
		{
			NODE minNode = MinNode(OpenList);

			ClosedList.push_back(minNode);

			next_x = minNode.X;
			next_y = minNode.Y;

			//delete the minimum node from the open list
			OpenList = DeleteElement(OpenList,next_x,next_y);
		}

	}
}

std::vector<NODE> FindPathAstar::BackTrack()
{
	vector<NODE> pathL;
	int k = 0;

	for(int i = ClosedList.size() - 1; i > -1; i--)
	{
		NODE tempNode;
		tempNode = GetNodeAt(ClosedList[i].ParentX,ClosedList[i].ParentY);
		if(tempNode.TotalCost != -1)
			pathL.push_back(tempNode);
	}
	return pathL;
}
NODE FindPathAstar::GetNodeAt(int x,int y)
{
	NODE tempNode;
	tempNode.TotalCost = -1;
	for(int i = 0; i < ClosedList.size(); i++)
	{
		if(ClosedList[i].X == x && ClosedList[i].Y == y)
		{
			tempNode = ClosedList[i];
		}
	}
	return tempNode;
}
NODE FindPathAstar::MinNode(vector<NODE> nodesList)
{
	NODE minNode;
	if(nodesList.size() > 0)
	{
		minNode = nodesList[0];
		for(int i = 1; i < nodesList.size(); i++)
		{
			if(nodesList[i].TotalCost < minNode.TotalCost)
				minNode = nodesList[i];
		}
		return minNode;
	}
	else
	{
		//return an error
		minNode.StartCost = -1;
		minNode.TotalCost = -1;
		minNode.X = 0;
		minNode.Y = 0;

		return minNode;
	}
}
std::vector<NODE> FindPathAstar::DeleteElement(vector<NODE> nodesList,int x,int y)
{
	vector<NODE> tempL = nodesList;

	for(int i = 0; i < tempL.size(); i++)
	{
		if(tempL[i].X == x && tempL[i].Y == y)
		{
			tempL.erase(tempL.begin() + i);
			break;
		}
	}
	return tempL;
}

bool FindPathAstar::IsElementExits(int x,int y)
{
	//check to see if the element is not exit in both lists or not
	for(int i = 0; i < OpenList.size(); i++)
	{
		if(OpenList[i].X == x && OpenList[i].Y == y)
		{
			return true;
		}
	}
	for(int i = 0; i < ClosedList.size(); i++)
	{
		if(ClosedList[i].X == x && ClosedList[i].Y == y)
		{
			return true;
		}
	}
	return false;
}
void FindPathAstar::ClearAll()
{
	OpenList.erase(OpenList.begin(), OpenList.begin() + OpenList.size());
	ClosedList.erase(ClosedList.begin(), ClosedList.begin() + ClosedList.size());
}